/*
//----------------------------------------------------------------------------
//
// (C) Copyrights Maritime Simulation Centre 2005 
//
//----------------------------------------------------------------------------
//
// $Source: $
//
// $Author: A. Abbing $
//
// $Contractor: MSCN$
//
// $Description$
//
// $End$
//
//----------------------------------------------------------------------------
*/

#ifndef __mml_Plane_h__
#define __mml_Plane_h__

#include "api.h"
#include <iostream>
#include <mst/Vector3.h>
#include <mst/Matrix4.h>
#include <mst/Math.h>
#include <mml/Vector.h>

namespace mml {

/*! 
\class Plane mml/Plane.h
\brief Geometric plane template class
\ingroup mmllin

A plane represents an infinite 2D plane as a normal and a distance
offset from the origin in the normal direction.  A point \f$ \vec{v} \f$ on the plane
satisfies the equation \f$ \vec{n} \cdot \vec{v} = offset \f$.
*/
 
template <class Type> class Plane {
public:
	/*!
	\typedef TYPENAME mst::Vector3<Type> Vector
	\brief The vector type definition
	*/
	typedef TYPENAME mst::Vector3<Type> Vector;

	/*!
	\fn typedef TYPENAME mst::Matrix4<Type> Matrix
	\brief The 4x4 matrix type definition
	*/
	typedef TYPENAME mst::Matrix4<Type> Matrix;
	
	enum IntersectionType 
	{
	    disjoint, coincide, intersect
	};

public:
	/*! 
	\fn Plane()
	\brief Construct a plane object
	*/
	Plane(): m_d(0.0), m_n() {}

	/*! 
	\fn Plane(Type a, Type b, Type c, Type d=0.0)
	\brief Construct a plane object
	*/
	Plane(Type a, Type b, Type c, Type d = 0.0) : m_d(0.0), m_n()
		{ this->set(a, b, c, d); }
	
	/*! 
	\fn Plane(const Vector &xyz, Type d = 0.0)
	\brief Construct plane object using plane normal and d
	*/
	Plane(const Vector &normal, Type d = 0.0): m_d(0.0), m_n() { 
		m_n = normal;
		m_n.normalize();
		m_d = d;
	}
	
	/*! 
	\fn Plane(const Vector &normal, const Vector &pos)
	\brief Construct plane object using plane normal and a position on the plane
	*/
	Plane(const Vector &normal, const Vector &pos)
		: m_d(0.0), m_n(normal) { 
		m_n.normalize();
		m_d = m_n.dot(pos);
	}

    /** From three points. */
    Plane(mml::Vector<3, Type> const& a, mml::Vector<3, Type> const& b, mml::Vector<3, Type> const& c)
      : m_d(0.0)
      , m_n()
    {
      mml::Vector<3, Type> normal = mml::cross(b - a, c - a);
      normal.normalize();

      m_n[0] = normal(0);
      m_n[1] = normal(1);
      m_n[2] = normal(2);

      m_d = a * normal;
    }

	/*! 
	\fn Plane(const Vector &a, const Vector &b, const Vector &c)
	\brief Construct a plane object using 3 positions on the plane
	*/
	Plane(const Vector &a, const Vector &b, const Vector &c) {
		Vector d1, d2;
		d1.sub(b, a);
		d2.sub(c, a);
		m_n.cross(d1, d2);
		m_n.normalize();
		m_d = m_n.dot(a);
	}

    /** mml version of the normal() function. */
    mml::Vector<3, Type> getNormal() const
    {
      return mml::Vector<3, Type>(m_n);
    }

	/*! 
	\fn virtual ~Plane()
	\brief Destruct a plane object
	*/
	virtual ~Plane() {}

	/*! 
	\fn void set(Type a, Type b, Type c, Type d=0.0)
	\brief Set/change the position values
	*/
	void set(Type a, Type b, Type c, Type d=0.0) {
		m_n.set(a, b, c);
		m_n.normalize();
		m_d = d;
	}
	
	/*!
	\fn bool operator ==(const Plane<Type> &p) const
	\param p The plane
	\brief Test for component-wise equality
	*/
	bool operator ==(const Plane<Type> &p) const
		{ return (m_n == p.m_n) && (m_d == p.m_d); }
		
	/*!
	\fn bool operator !=(const Plane<Type> &p) const
	\param p The plane
	\brief Test for component-wise un-equality
	*/
	bool operator !=(const Plane<Type> &p) const
		{ return (m_n != p.m_n) || (m_d != p.m_d); }
	
	/*!
	\fn bool almostEqual(const Plane<Type> &p, Type tol) const
	\param p the second plane
	\param tol the tolerance value
	\brief Tests for approximate component-wise equality

	Tests for approximate component-wise equality of
	two planes. It returns false or true depending on whether
	the absolute value of the difference between each pair of 
	components is less than the tolerance. 
	*/
	bool almostEqual(const Plane<Type> &p, Type tol) const {
		return m_n.almostEqual(p.m_n, tol) && mst::equal(m_d, p.m_d, tol);
	}

	/*!
	\fn Type& operator[](int i)
	\param i index
	\brief Accesses indexed component of plane
	*/
    Type& operator[](int i)
    {
      return (i < 3) ? m_n[i] : m_d;
    }

	/*!
	\fn const Type& operator[](int i) const
	\param i index
	\brief Accesses indexed component of plane
	*/
    Type const& operator[](int i) const
    {
      return (i < 3) ? m_n[i] : m_d;
    }
	
	/*! 
	\fn void closest(const Vector &pt, Vector &dst) const
	\brief Compute closest point \a dst on plane to point \a pt
	*/
	void closest(const Vector &pt, Vector &dst) const {
		Vector ptrel, origin;
		origin.scale(m_d, m_n);
		ptrel.sub(pt, origin);
		dst.addScaled(pt, -ptrel.dot(m_n), m_n);
	}
	
	/*! 
	\fn void orthoXform(const Plane<Type> &pln, const Matrix &m)
	\brief Transform the plane \a pln with matrix \a m
	*/
	void orthoXform(const Plane<Type> &pln, const Matrix &m)
	{
		Vector pt, b;
		pt.scale(pln.m_d, pln.m_n);
		m_n.xformVec(pln.m_n, m);
		b.xformPt(pt, m);
		m_n.normalize();
		m_d = m_n.dot(b);
	}
	
	/*! 
	\fn bool inHalfSpace(const Vector &pt) const
	\brief Test if point \a pt on positive side of the plane
	*/
	bool inHalfSpace(const Vector &pt) const {
		return pt.dot(m_n) <= m_d;
	}

	/*! 
	\fn Vector normal() const
	\brief returns normal of plane
	*/
	Vector const& normal() const {
    return m_n; 
  }

  /**
   * Return the distance of the plane to the origin.
   */
  Type const& distance() const {
    return m_d;
  }
  
  /*
   * Determine intersection of two planes
   */
  IntersectionType intersection(const mml::Plane<Type> &plane2, mst::Vector3<Type> *pV1, mst::Vector3<Type> *pV2) const
  // The 3D intersection of two planes by Dan Sunday
  // http://geometryalgorithms.com/Archive/algorithm_0104/algorithm_0104B.htm
  // note: in the original version, the planes are given by a point plus a normal vector
  // input : other plane is plane2 (this plane is plane1)
  // output: the intersection line, if it exists, as given by two points on this line pV1 and pV2
  // adapted and added to mml by Anneke Sicherer-Roetman
  // TODO make only one output parameter of type Line3<Type> but this seems to clash somewhere ASR
  {
    // compute point per plane from normal and distance
    mst::Vector3<Type> planePoint1 = distance() * normal();
    mst::Vector3<Type> planePoint2 = plane2.distance() * plane2.normal();
    
    // compute cross product of both planes; this gives the direction of the intersection line
    mst::Vector3<Type> u;
    u.cross(normal(), plane2.normal());
    Type ax = (u[0] >= 0 ? u[0] : -u[0]);
    Type ay = (u[1] >= 0 ? u[1] : -u[1]);
    Type az = (u[2] >= 0 ? u[2] : -u[2]);

    // test if the two planes are parallel
    if ((ax + ay + az) < 0.00000001) // plane1 and plane2 are near parallel
    {
        // test if disjoint or coincide
        mst::Vector3<Type> v = planePoint2 - planePoint1;
        if (v.dot(normal()) == 0) // plane2.V0 lies in plane1
            return coincide;      // plane1 and plane2 coincide
        else
            return disjoint;      // plane1 and plane2 are disjoint
    }

    // plane1 and plane2 intersect in a line
    // first determine max abs coordinate of cross product
    int maxc; // max coordinate
    if (ax > ay) 
    {
        maxc = (ax > az) ? 1 : 3;
    }
    else 
    {
        maxc = (ay > az) ? 2 : 3;
    }

    // next, to get a point on the intersect line,
    // zero the max coord, and solve for the other two
    mst::Vector3<Type> iP; // intersect point
    Type d1 = -planePoint1.dot(normal());
    Type d2 = -planePoint2.dot(plane2.normal());

    switch (maxc) 
    {
    case 1:                    // intersect with x=0
        iP[0] = 0;
        iP[1] = (d2*normal()[2] - d1*plane2.normal()[2]) / u[0];
        iP[2] = (d1*plane2.normal()[1] - d2*normal()[1]) / u[0];
        break;
    case 2:                    // intersect with y=0
        iP[0] = (d1*plane2.normal()[2] - d2*normal()[2]) / u[1];
        iP[1] = 0;
        iP[2] = (d2*normal()[0] - d1*plane2.normal()[0]) / u[1];
        break;
    case 3:                    // intersect with z=0
        iP[0] = (d2*normal()[1] - d1*plane2.normal()[1]) / u[2];
        iP[1] = (d1*plane2.normal()[0] - d2*normal()[0]) / u[2];
        iP[2] = 0;
    }
    
    *pV1 = iP;
    *pV2 = iP + u;
    return intersect;
}
	
private:
	Type m_d;	// Plane offset
	Vector m_n;	// Plane normal
};

/*! 
\typedef Planef
\brief float specialisation of a Plane
\ingroup mmllin
*/
typedef Plane<float> Planef;

/*! 
\typedef Planed
\brief Double precision specialisation of a Plane
\ingroup mmllin
*/
typedef Plane<double> Planed;

//template<class Type>
//std::ostream& operator<<(std::ostream &s, const mml::Plane<Type> &p)
//{
//	s << p[0] << " " << p[1] << " " << p[2] << " " << p[3];
//	return s;
//}

template<class Type>
std::ostream& operator<<(std::ostream &s, const mml::Plane<Type> &p)
{
	s << p.normal() << " " << p.distance();
	return s;
}

}

#endif

